/**
 * @file LESO.cpp
 */

#include <LESO.hpp>

#include <mathlib/math/Functions.hpp>
using namespace matrix;
LESO::LESO(){
	set_wo(5);
}

matrix::Vector3f  LESO::get_e_estimate(){
	// matrix::Vector3f e;
	// e(0) = Z[0](0);
	// e(1) = Z[1](0);
	// e(2) = Z[2](0);
	// return e;
	return {Z[0](0), Z[1](0), Z[2](0)};
}

matrix::Vector3f  LESO::get_rate_estimate(){
	// matrix::Vector3f rate;
	// rate(0) = Z[0](1);
	// rate(1) = Z[1](1);
	// rate(2) = Z[2](1);
	// return rate;
	return {Z[0](1),  Z[1](1),  Z[2](1)};
}

matrix::Vector3f  LESO::get_disturbance_estimate(){
	matrix::Vector3f disturbance;
	disturbance(0) = Z[0](2) /coef(0) ;
	disturbance(1) = Z[1](2) /coef(1) ;
	disturbance(2) = Z[2](2) /coef(2);
	return disturbance;
}
void LESO::set_wo(const float & _wo)
{
	wo = _wo;
	float l11 = 1.5f*wo;
	float l21 = 1.5f*wo*wo;
	float l31 = 0.5f*wo*wo*wo;
	float l12 = -15.0f/14;
	float l22 = 1.5f*wo;
	float l32 = 9.0f/14*wo*wo;
	A(0,0) = -l11;	A(0,1) = -1-l12;	A(0,2) =  0;
	A(1,0) = -l21;	A(1,1) = -l22;	A(1,2) = 1;
	A(2,0) = -l31;	A(2,1) = -l32;	A(2,2) = 0;

	B(0,0) = 0;  B(0,1) = l11;  B(0,2) = l12;
	B(1,0) = 1;  B(1,1) = l21;  B(1,2) = l22;
	B(2,0) = 0;  B(2,1) = l31;  B(2,2) = l32;
}
void LESO::update_phi_and_tau(const float dt){
	PHI =  A * dt;
	PHI(0,0) = PHI(0,0) + 1.0f;
	PHI(1,1) = PHI(1,1) + 1.0f;
	PHI(2,2) = PHI(2,2) + 1.0f;

	TAU = B *dt;
}

void LESO::set_aux_para(const matrix::Vector3f &proportional_gain, const float yaw_weight)
{
	att_p = proportional_gain;
	float _yaw_w = math::constrain(yaw_weight, 0.f, 1.f);
	// compensate for the effect of the yaw weight rescaling the output
	if (_yaw_w > 1e-4f) {
		att_p(2) /= _yaw_w;
	}
}
void LESO::set_last_U(const matrix::Vector3f &u){
	last_U = u;
}
matrix::Vector3f LESO::calculate_e(const matrix::Vector3f &rate_sp){
	matrix::Vector3f e = rate_sp.edivide(att_p)*0.5f;
	float e_norm = e.norm();
	float alpha =2.0f *  asinf(e_norm);
	e = e / e_norm * alpha;
	return e;
}
void  LESO::run(const matrix::Vector3f &rate_sp, const matrix::Vector3f &rate, const float dt){
	update_phi_and_tau(dt);
	//1、从rate_sp中得到e
	matrix::Vector3f e = calculate_e(rate_sp);
	//2、run
	matrix::Vector3f  Z_next[3];
	for(int i = 0; i < 3; i++){
		matrix::Vector3f  U{last_U(i) * coef(i), e(i), rate(i)};
		Z_next[i] =  PHI*Z[i] + TAU*U;
		// Z[i] = PHI*Z[i] + TAU*U;
	}
	for(int i = 0; i < 3; i++){
		Z[i] = Z_next[i];
	}
	//3、publish话题
	leso_value_s  leso_status{};
	leso_status.timestamp = hrt_absolute_time();

	leso_status.ex_real =  e(0);
	leso_status.ey_real =  e(1);
	leso_status.ez_real =  e(2);
	leso_status.ex_esti =  Z[0](0);
	leso_status.ey_esti =  Z[1](0);
	leso_status.ez_esti =  Z[2](0);

	leso_status.wx_real =  rate(0);
	leso_status.wy_real =  rate(1);
	leso_status.wz_real =  rate(2);
	leso_status.wx_esti =  Z[0](1);
	leso_status.wy_esti =  Z[1](1);
	leso_status.wz_esti =  Z[2](1);

	leso_status.dx =  Z[0](2);
	leso_status.dy =  Z[1](2);
	leso_status.dz =  Z[2](2);
	_leso_value.publish(leso_status);
}
void LESO::reset(){
	for(int i = 0; i < 3; i++)
		for(int j = 0; j < 3; j++)
			Z[i](j) = 0.0f;
}
